#ifndef STUDIO_PID_H
#define STUDIO_PID_H

#include "studio_macros.h"

class studio_pid
{
public:
    // 构造函数（初始化限幅值为默认值）
    studio_pid(double init_v, double goal_v, double p, double i, double d,
               double integral_limit_val = 1e3, double output_limit_val = 1e3)
        : initval(init_v), goal(goal_v), Kp(p), Ki(i), Kd(d),
          error(goal - initval), prev_error(error),
          inte_error(0.0), dif_error(0.0),
          integral_limit(integral_limit_val), output_limit(output_limit_val)
    {
    }

    // 合并更新误差、计算总误差和返回PID控制器输出的功能

    /// 计算PID输出
    /// @param current_value 当前值
    /// @return PID控制输出值
    double compute(double current_value)
    {
        // 计算当前误差 CTE
        double cte = goal - current_value;

        // 更新积分和微分项
        inte_error += cte; //* dt;                // 积分项乘以时间间隔
        dif_error = (cte - prev_error); // / dt;   // 微分项除以时间间隔
        prev_error = cte;

        // 应用积分限幅

        if (inte_error > integral_limit)
        {
            inte_error = integral_limit;
        } else if (inte_error < -integral_limit)
        {
            inte_error = -integral_limit;
        }
        // 计算PID输出
        double output = Kp * cte + Ki * inte_error + Kd * dif_error;
        // 应用输出限幅
        if (output > output_limit)
        {
            output = output_limit;
        } else if (output < -output_limit)
        {
            output = -output_limit;
        }

        return output;
    }

    /// 设置积分限幅
    /// @param limit
    void set_integral_limit(double limit)
    {
        integral_limit = limit;
    }

    /// 设置输出限幅
    /// @param limit
    void set_output_limit(double limit)
    {
        output_limit = limit;
    }

    /// 获取目标值
    /// @return
    double get_goal() const
    {
        return goal;
    }

    /// 设置目标值 （会自动清除之前的积分和微分状态）
    /// @param goal_v
    void set_goal(double goal_v)
    {
        goal = goal_v;
        reset(); // 清除之前的积分和微分状态
    }

    // 设置PID参数
    void set_pid_params(double p, double i, double d)
    {
        Kp = p;
        Ki = i;
        Kd = d;
    }


    void reset()
    {
        prev_error = goal - initval;
        inte_error = 0.0;
        dif_error = 0.0;
    }

private:
    double initval; // 初始值
    double goal; // 目标值
    double error; // 当前误差（目标值 - 当前值）
    double Kp; // 比例常数
    double Ki; // 积分常数
    double Kd; // 微分常数
    double prev_error; // 前一个误差
    double inte_error; // 积分误差
    double dif_error; // 微分误差
    double integral_limit; // 积分项限幅值
    double output_limit; // 输出限幅值
};

#endif  // STUDIO_PID_H
